• 专利附录
  • 专利说明
  • 权利要求
  • 类似技术
  • 同族专利
  • 引用文献
  • 法律状态
  • 优先权
    • 专利摘要:
    There is provided a method for tracking the navigation of a mobile carrier, wherein an extended Kalman filter estimates during successive iterations a carrier's navigation state, an iteration of the filter comprising steps of: propagating a state preceding navigation of the carrier in a propagated state according to a kinematic model and / or measurements acquired by at least one inertial sensor, updating the propagated state as a function of measurements acquired by at least one navigation sensor, in which, for at least one carrier navigation variable contained in the propagated state, the update comprises sub-steps of: calculating a first corrective term expressed in a fixed reference with respect to the carrier, calculation of a second corrective term expressed in an inertial frame in which the wearer is mobile, by a reference change applied to the first corrective term, and addition of the second corrective term to the value of the variable contained in ue in the propagated state, the state containing the result of this addition being used as the output state of the iteration.
    公开号:FR3034514A1
    申请号:FR1500654
    申请日:2015-04-01
    公开日:2016-10-07
    发明作者:Axel Barrau；Silvere Bonnabel
    申请人:Sagem Defense Securite SA；
    IPC主号:
    专利说明:

    [0001] FIELD OF THE INVENTION The invention relates to the field of navigational tracking of a mobile carrier from incomplete or noisy measurements. The invention more particularly relates to a navigation tracking method of a mobile carrier in which is implemented an extended Kalman filter of particular type. STATE OF THE ART A Kalman filter is a well-known tool for tracking a carrier such as a ship, an aircraft or a land vehicle, ie its position, speed, acceleration, etc. . During successive iterations, the Kalman filter estimates a navigational state of the carrier via matrix equations, thus linear, by means of noisy measurements provided by navigation sensors.
    [0002] The bearer is then assimilated to a dynamic system governed by linear equations, which constitutes a constraining limitation. To extend the Katman filter to dynamical systems governed by nonlinear equations, a method referred to as the Extended Kalman Filter (EKF) has been proposed. This evolution proposes an additional step of linearizing, at each new iteration of the filter, the equations governing the non-linear system at a point in the vector space, this point being typically an estimated state during a previous iteration. The matrices resulting from this linearization can thus be used to calculate a new state estimated according to the conventional Kalman filter method. However, the known extended Kalman filters have the disadvantage of not functioning correctly if the linearization point is too far from the actual state of navigation of the carrier. However, in certain navigation tracking contexts, no accurate estimate of the state of navigation of the carrier is available at the start of the filter, so that the implementation of the successive iterations of the extended Kalman filter does not make it possible. not converge towards an accurate estimate of the state. To solve this problem in the particular context of the alignment of a carrier, it has been proposed a first method, comprising three phases: a search for vertical, then a first alignment providing a relatively rough estimate on the basis of the vertical, then a second alignment using the rough estimate to obtain a more accurate estimate. This method suffers from numerous drawbacks: it is difficult to manage, must be adapted to the context in question, the movements of the wearer are constrained during the alignment phase, and the implementation in three successive phases slows down the convergence. Also, the applicant has proposed in your application FR 1401512 15 filed July 4, 2014 a second method responding to the aforementioned Einerisation problem, this method proposing to implement two parallel processing: a basic processing including certain changes of variables allowing the implementation A particular extended Kalman filter, referred to as an "invariant" Kalman filter, and a simplified processing estimating the state of the true system by a first-order expansion around the estimate of the invariant filter. Both treatments work simultaneously in parallel. The second method converges more rapidly than the first method, and the difficulties associated with the transitions encountered in the implementation of the first method are eliminated. However, the two processing levels of the second method are complex to coordinate in real time. The algorithm is difficult to fit into an architecture designed for a classical extended Kalman filter. In addition, the behavior of this second method in the case where the errors of the sensors (bias / scale factors / timing) are high is difficult to predict.
    [0003] GENERAL OBJECTIVE OF THE INVENTION An object of the invention is to estimate a navigation state of a carrier governed by a system of nonlinear equations, by means of an estimator capable of converging even when this estimator is configured with initial conditions remote from the actual navigation state of the carrier. Another object of the invention is to provide a method which is more suitable for real-time execution than the prior art solutions presented above. It is proposed that a mobile carrier navigation tracking method, in which an extended Kalman filter estimates during successive iterations a carrier navigation state, an iteration of the filter comprising steps of: - propagation of a previous navigation state of the carrier in a propagated state according to a kinematic model and / or measurements acquired by at least one inertial sensor, - update of the propagated state as a function of measurements acquired by at least one navigation sensor, wherein, for at least one carrier navigation variable contained in the propagated state, the update comprises sub-steps of: calculating a first corrective term expressed in a fixed reference with respect to the carrier, - calculation of a second corrective term expressed in an inertial frame in which the carrier is mobile, by a change of mark applied to the first corrective term, - addition of the second corrective term to the value of the a variable contained in the propagated state, the state containing the result of this addition being used as the output state of the iteration.
    [0004] The step of updating this method is unconventional compared to those encountered in the methods of the state of the art based on extended Kalman filter. This modified update step defines a correction term in the bearer mark which has the effect of removing the linearization problems mentioned above. In other words, the extended Kalman filter can converge to an accurate estimate even when the linearization is performed at a point too far from the actual state of the carrier. Furthermore, it is found that the proposed method does not require the simultaneous execution of two parallel treatments; it is therefore more suited to real-time constraints than the method described in application FR 1401512, despite a comparable speed of convergence. The process may also include the following features taken alone or in combination where technically possible. The updating step may comprise in particular: the calculation of a linear correction term from an innovation representative of a difference between the measurements acquired by the navigation sensor and the propagated state, and calculating an exponential of the linear correction in the sense of a Lie group, the first corrective term depending on said exponential. The linear correction can be equal to the innovation multiplied by a Kalman gain. The computation of the second corrective term may include a multiplication of a rotation matrix with the first corrective term. The method may further include adjusting at least one of the correction terms, prior to its use by the filter, by means of an aging transformation of said correction term during the iteration time of the extended Kalman filter.
    [0005] The adjustment of the corrective term includes substeps of: - additional correction of the corrective term from a stored deviation representing a cumulative approximation error in carrying out an aging transformation of at least 5 a previous iteration of the filter, - application of the aging transformation to the corrected offset term. The adjustment of a first corrective term can be implemented by means of a transformation comprising a base change of the first corrective term, the destination base being invariant by the action of a group. The adjustment of such a first corrective term may comprise a transformation of the linear correction term E of the following form: E (from where cI) is a matrix of which at least one block is calculated by the formula Ad (Xt + At) -1FAd (Xt), Ad (.) Being an adjoint matrix in the sense of the Lie group theory, P a propagation matrix, and At an iteration duration of the extended Kaiman filter. F. ' may be one of the following forms: 13 03.3 03.3 f g 13 03.3; OR f f g AT. 13 13 - 20 - / 03.39 / 3); or - P = 16; where - f g denotes the integral of the gravity vector calculated at an estimated position of the carrier between the instants t and t + At; G is the double integral of the gravity vector calculated at an estimated position of the wearer between times t and t + Δt; - 13 denotes an identity block of order 3, 3034514 6 - 03,3 denotes a square block of order 3 whose terms are zero. The propagation step may further implement a Riccati equation of the following form: Pt + At CePt (1) T ± At. Q where (1). is a matrix of which at least one of the blocks is calculated by the formula Ad (iem) -1PAd (gt), Ad (.) being an adjoint matrix in the sense of the Lie group theory, F 'is a matrix propagation, and At is an iteration time of the extended Katman filter. The process steps may be implemented by a plurality of Kalman filters configured with different initial conditions, and operating in parallel so as to produce a plurality of candidate estimates of the carrier's navigation state; in this case, the method may further comprise generating a consolidated estimate of the carrier's navigation state from the plurality of candidate estimates, the consolidated estimate depending on a comparison between each candidate estimate and the measurements acquired by the navigation sensor. At least one of the navigation sensors may be a receiver of radionavigation signals emitted by satellites. According to a second aspect, the invention further provides a computer program product comprising program code instructions for performing the steps of the preceding navigation tracking method. According to a third aspect, the invention proposes a mobile carrier navigation tracking device comprising: a reception interface for measurements acquired by at least one inertial sensor; a reception interface for secondary measurements acquired by at least one a navigation sensor, at least one processor configured to implement an extended Kalman filter during successive iterations to estimate a carrier's navigation state, an iteration of the filter comprising steps of: o propagation of a previous navigation state in a propagated state according to a kinematic model and / or primary measurements, o updating the propagated state according to the navigation measurements, the processor being further configured to update at least one propagated state navigation variable with substeps of: calculating a first corrective term expressed in a fixed reference with respect to the carrier, l of a second corrective term expressed in an inertial frame in which the carrier is mobile, by a change of reference applied to the first corrective term, 15 - addition of the second corrective term to the value of the variable in the propagated state, state containing the result of this addition being used as the output status of the iteration. It is also proposed a mobile carrier navigation unit, comprising at least one inertial sensor, at least one navigation sensor, and at least one navigation tracking device according to the third aspect. At least one navigation sensor may be a receiver of radionavigation signals emitted by satellites.
    [0006] DESCRIPTION OF THE FIGURES Other characteristics, objects and advantages of the invention will emerge from the description which follows, which is purely illustrative and nonlimiting, and which should be read with reference to the accompanying drawings, in which: FIG. 1 shows schematically a mobile carrier, and two markers involved in calculations implemented by a navigation unit according to one embodiment of the invention, - 2 shows a navigation unit according to one embodiment of the invention for a mobile carrier, FIG. 3 illustrates the steps of a mobile carrier navigation tracking method by means of an extended Kalman filter, according to one embodiment of the invention, FIG. 4 details the substeps of an updating step included in the method of FIG. 3; FIG. 5 is a timing diagram illustrating instants of acquired measurement arrivals and processing times; operated by a navigation unit. In all the figures, similar elements bear identical references. DETAILED DESCRIPTION OF THE INVENTION Referring to FIG. 1, a carrier A is movable in an inertial reference frame Ri. Carrier A is here a ship but can alternatively be an aircraft, or a land vehicle. An example of an inertial landmark Ri is the landmark centered on the center of the Earth, whose z axis points to the north pole, whose x axis points to the intersection of the Greenwich meridian and the equator at the time. t = 0 (the point thus defined will then move in our frame because of the rotation of the Earth) and whose axis y points in the direction of the vector zxx, x designating the vector product). A reference Rp attached to the carrier A is defined. Rp is referred to as the carrier mark or measurement mark in the following. For example, the axes of the bearing mark used most frequently are directed to the front, to your right, and to the bottom of the unit. Its origin is a fixed point of the wearer. With reference to FIG. 2, a navigation unit 1 comprising at least one primary sensor 11, at least one secondary sensor 12, and a navigation tracking device 2 is embarked on the carrier A.
    [0007] The navigation unit 1 is fixed to the structure of the carrier A; consequently, the central station 1 is immobile in the reference point of the carrier Rp. In the following, the nonlimiting example of a hybrid GNSS / inertial navigation center 1 is taken.
    [0008] This hybrid GNSS / inertielte navigation station 1 comprises at least six primary sensors 11 of the inertial sensor type: three accelerometers and three gyrometers. The inertial sensors 11 are configured to acquire measurements in the reference frame of the carrier Rp.
    [0009] The navigation unit 1 also comprises, as secondary sensor 12, at least one navigation signal receiver emanating from satellites S (GNSS, GPS or equivalent). The navigation tracking device 2 comprises a primary interface 21 for receiving measurements acquired by the primary sensors 11, a secondary interface 22 for receiving measurements acquired by the secondary sensors 12, and at least one processor 20 configured to implement an extended Katman filter (generally referred to by the acronym EKF in your literature). The extended Kalman filter EKF is an algorithm capable of being encoded as a computer program executable by the processor 20. The navigation tracking device 2 further comprises an output 23 for outputting output data. calculated by the processor 20.
    [0010] Extended Kalman Filter In known manner, an extended Kalman filter EKF is a recursive estimator of a state representative of the carrier's navigation, hereafter referred to as the navigational state. This navigation state may include at least one carrier navigation variable (position, speed, acceleration, orientation, etc.). The navigation state may in any case be represented as a vector, each component of which is a navigational variable of the carrier. In the following an embodiment is considered in which the navigation state comprises the following navigation variables: a vector position x of the carrier of dimension 3, a velocity vector y of P of dimension 3, A matrix of R orientation of the carrier, defined as the rotation matrix for the change of marker from the carrier mark Rp to the inertial mark Ri.
    [0011] The state furthermore comprises N additional variables which may be, for example, disturbances of the sensor measurements: a vector of dimension 3 containing the drifts of the gyrometers in reference to the carrier Rp, a vector of dimension 3 containing the biases accelerometers in the frame of the carrier Rp, - a dimension vector 9 containing scale factors and gyrometer calibration errors, - a dimension vector 9 containing scale factors and accelerometer calibration errors. In this case, we have N = 24. The additional variables are gathered in a vector of size N and noted b. The estimated magnitudes will be noted in the following with hats (2, fy, b) and real quantities without hats (x, v, R, b).
    [0012] The extended EKF Katman filter is configured with a kinematic model of the A-carrier. This model is typically representable by a nonlinear propagation function. This propagation function models the evolution of the navigation variables of the carrier's navigation state between two instants.
    [0013] The Kalman EKF filter is further configured with an observation model, or observation function, which models the behavior of the secondary sensors used (here the or each GNSS receiver). The observation function makes it possible to take into account measurement errors made by the secondary sensors. The observation function can be non-linear.
    [0014] The extended Kalman filter is implemented by successive iterations. The filter is initialized with an initial state, which will serve as input for a first iteration of the filter. Each subsequent iteration of the filter takes as input a state estimated by a previous iteration of the filter, and provides a new estimate of the state of the carrier. With reference to FIG. 3, an iteration of the extended Kalman filter conventionally comprises three steps: a linearization 100, a propagation 300, and an update 500. The linearization step 100 is implemented in accordance with the description which is given in the introduction. It is recalled here that this step 100 uses a previous state as a point of iinéarisation, so as to approximate the functions of propagation and observation by linear functions. These linear functions are then used during propagation steps 300 and 500.
    [0015] The propagation step 300 determines a propagated state of the carrier from the carrier's previous state (or initial state for your first iteration), using the linearized propagation function. Furthermore, propagation 300 produces a covariance matrix P representative of an uncertainty in the acquired measurements.The propagation 300 implements a Riccati equation of the following form: Pt + AT (1) Pt (1) 7 ± LIT. Q where Q is the covariance matrix of the model noise defined by the measurement uncertainty of the different sensors, At is an iteration time of the extended Kalman filter, and ci) is the solution in t + At of a 3034514 12 differential equation of the form (1) t = Id, c ± d = Fs (13 'Fs being your first-order error propagation matrix, all the errors of the motion variables being projected into the reference frame attached to the estimated state of the carrier Here we have: 5 Where is defined at the first order by fiTR A () with: ul 0 -U3 U2A (112). (u3 0 -u1 u3 -U2 U1 0 The Riccati equation can also take the more classical form between t and t + At: d FtP + PFT + Q In an optimized form of computation, one of the blocks of cl) is computed without recourse to a differential equation, by the formula: c1) = Acet + At) -1FAd (gt), where gt is your estimated value of the variables of movement, Ad (.) is an adjoint matrix in the sense of Lie group theory and F 'is a propagation matrix. According to a first variant, the matrix -E has the following form: ## EQU1 ## According to a second variant, F 'has the following form: ## EQU2 ## According to a third variant, has the following form: = 6 The propagation equations can involve the primary measurements acquired 200 by the primary sensors 11. Secondary measurements YGps are measured by the secondary sensors 12 in step 400, these measurements being expressed in the reference of the carrier Rp. The YGps measurements are received by the secondary interface 22 which transmits them to the processor 20 for processing.
    [0016] In the update step 500, the processor corrects the propagated state generated by the propagation step 300, using the linearized observation function and using the secondary measurements provided by the secondary sensors. in step 400. The updated status produced by this step 500 is hereinafter referred to as g +.
    [0017] The updated state g + is provided on the output 23. It should be noted that, in the implementation of the extended Kalman filter considered here, two types of measurement intervene respectively in the propagation 300 and update 500 steps (FIG. the measurements provided by the primary sensors 11 and secondary 12).
    [0018] This operation is typical of a hybrid plant, in which your secondary measures are used to consolidate the primary measures. It is nevertheless possible to predict that the propagation step does not use measurements, but only a kinematic model to propagate the previous state in the propagated state g. In this case, only the 20 measurements provided by the secondary sensors 12 are used by the extended Kalman filter during the update 500. The steps 100 to 500 are then repeated at each iteration of the extended Kalman filter executed by In one embodiment, the processor 20 executes several parallel extended Kalman filters, which use the same primary and secondary measurements. However, the various extended Kalman filters operating in parallel are initialized with different initial conditions (these initial conditions are in this case the selected variable values in the state which serves as a linearization point during the step of linearization of your first iteration of each filter).
    [0019] Each Kalman filter therefore provides a candidate estimate of the state of the distinct carrier. A consolidation step can then be implemented producing a consolidated estimate of the carrier's navigation state from the candidate estimates, the consolidated estimate depending on a comparison between each candidate estimate and the measurements acquired by the secondary sensor. . The consolidation step can, for example, select one of the candidate estimates as a consolidated estimate. Alternatively, several candidate estimates are computationally combined to produce the consolidated estimate. We will now detail more precisely the substeps of update step 500, with reference to FIG. 3. In a step 501, processor 20 calculates an innovation Z representative of a difference between the YGps measurements acquired by the secondary sensors 12 and the propagated state g. Innovation is calculated in the wearer's landmark. If the secondary measurement is a position, the innovation is typically calculated using the following formula: Z = fiT (YGps- The processor 20 further calculates a K gain in the Kalman sense using the following equations: S = HPHT + EK = PHT S-1 In the equations above, E is the assumed covariance matrix of the measurement noise of the secondary sensors in the carrier's frame of reference (it can depend on the estimated state) and H is defined by a 25 first-order development where all the errors concerning the magnitudes of movement are expressed in the reference of the carrier: 3034514 15 flTX fiT £ + H In the case of position observations coming from a GPS receiver we will have H = (03.6 13 03, N) The covariance P produced by the propagation step is also updated 502 in a conventional manner, as follows: P + = (Id - KH) P 5 By convention, your notation + "is in the this document to the output data of the 500 update P + here denotes the covariance produced by the update. In a step 503, a linear correction is calculated as a function of the Kalman K gain and the Z innovation.
    [0020] 10 The linear correction is equal to the innovation multiplied by a gain of Kalman, as you express the formula below: (c / X) = K. Z Ub The linear correction, which is the term of left in the equation above, consists of a vector dX corresponding to the variables of motion (position, speed, attitude for example) and a vector db 15 corresponding to the other variables, each in relation with variables of the propagated state Î . For at least one of the variables of the propagated state concerned by the vector dX, the processor 20 calculates 504 a first corrective term expressed in the fixed carrier frame Rp with respect to the carrier.
    [0021] For example, a first corrective term eR relating to the orientation matrix f 1 is calculated; a first corrective term ev relating to the velocity vector i3; a first corrective term ex relating to the position vector 3034514 Each first corrective term depends on the Lie group exponential of the linear correction. The set of first corrective terms results, for example, from the following matrix computation: ev ex x03 -X3 X2 X4 X7 -X1 X5 X8 (eR 013 1 0 = exp -X2 X1 O X6 X9 01.3 0 1 0 0 0 0 In this matrix equation, the left-hand matrix comprises your first corrective terms, the terms xi are the components of the sub-vector dX (which is here of dimension 9 because it refers to the position vector of dimension 3, to 3-dimensional velocity vector and the orientation matrix); the operator "exp" denotes either an accurately calculated matrix exponential or an approximate matrix exponential of order n> 1, i.e. that is, only the first n terms m m2 m3 of the series 1 + -1! + -2! + -3! ± defining the exponential of a matrix M are calculated, preferably the order n is greater than 2.
    [0022] The processor 20 then proceeds to the calculation 505 of a second corrective term expressed in the inertial reference frame Ri in which the wearer is mobile, by a change of reference applied to a corresponding first corrective term, which is expressed to him in the reference of the wearer. Rp. Three corrective terms SR, Sv, Sx are obtained by this calculation. A second corrective term SR relating to the orientation matrix f 1; a second corrective term Sv relating to the velocity vector i3; a second corrective term Sx relating to the vector position 2. The calculation of the second corrective terms is for example the following: (5R = eR Sv = Sx = R. ex 3034514 17 It is noted here that your first terms e and ex, The respective speed and position of the carrier are multiplied on the left by the index change matrix F. For each second corrective term, the processor adds 506 the second corrective term calculated to the value of the variable contained in the propagated state, as follows: = Ê.SR V ± = f + 8v + 8x The additional variables are updated in the usual way: h + = + db Finally, the update step whose sub-steps come from be detailed transforms a propagated state ie containing the variables fi, 10 f), î and an updated state g + containing updated variables fi +, the, .2 'and b + taking into account the secondary measures (here of type GNSS) provided by your secondary sensors 12 In the update step 500 which has just been described, it is found that the linear correction is not directly added in step 506 in propagated state g. Firstly, the (second) correction terms which are used for the addition 50 are expressed in the inertial reference frame Ri (after application of the step 504), which makes it possible to benefit from advantages similar to those obtained by using a invariant Kalman filter, as defined in the article "The invariant extended Kalman filter as stable observe, A. Barrau and S. Bonnabel, Arxiv preprint, arXiv: 1410.1465". However, the marker change is here implemented directly during update step 500 of the extended Kalman filter; this change is therefore easier to implement in an existing navigation system, and operating in real time. Secondly, the (second) corrective terms used for the additive update performed in step 506 are from an exponential transformation step (step 504), which has the effect of accelerating your filter convergence. of extended Kalman. Aging data used by the Kalman filter 5 In the navigation unit 1 operating on the basis of primary sensors 11 and secondary sensors 12, the primary sensors 11 and the secondary sensors 12 do not necessarily operate at the same rate, so that The average period of reception of measurements acquired by the primary sensors 11 may be different from the average period of reception of measurements acquired by the secondary sensors 12. For example, it is conventional for an inertial hybrid / GNSS plant to operate with an average period of time. GNSS receiver provided by the GNSS receiver 12 equal to 3.84 seconds, and the average receiving period of inertial increments provided by the inertial sensors much lower (the arrival frequency of the inertial increments is in other terms much faster than the arrival frequency of GNSS measurements). The extended Kalman filter is conventionally configured to trigger its update calculations 500 according to the arrival rate of the GNSS secondary measurements. This phenomenon is illustrated in FIG. 5. In this figure, each square schematically represents the processing of new inertial increments Av, AO provided by the primary sensors 25 of the inertial type 11. In an ideal system in which the calculation calculations 500 days would be instantaneous, that is to say of zero duration, the updated state Î + would be available from the instant t of arrival of the inertial increments immediately following the arrival time of the GNSS measurements which have served for the 30 update calculations (this ideal availability being represented by the dashed arrow in FIG. 4).
    [0023] In fact, the updating calculations 500 last in practice a significant duration. Consequently, the primary inertial measurements may have caused the estimated state to evolve during the course of the update calculations 500, so that the result of the update 500 is in practice obsolete with respect to the propagated estimate. To work around this problem of obsolescence, and thus improve the accuracy of the estimates calculated by the extended Kalman filter, it is possible to adjust the value of some of the correction terms involved in the calculations of the update 500, this adjustment having the function of simulating the evolution, or aging of these terms during the duration of the 500 update computations. By convention, the term "aging transformation" is defined as a transformation converting a corrective term into an adjusted corrective term during the duration of an iteration of the extended Kalman filter (i.e., the time between triggering the update of a given iteration, and triggering the update of the subsequent iteration ). A first aging transformation can be applied, for example, to at least one of the first corrective terms eR, ei ex 20 (that is, the correction terms resulting from the exponential computation made on the basis of the linear correction depending on the innovation Z). The first transformation of a first corrective term preferably comprises a base change of the first corrective term considered, the destination base of this change being chosen invariant by the action of a group. Such a group is for example described in the document - The invariant extended Kalman filter as stable observe, A. Barrau and S. Bonnabel, Arxiv preprint, arXiv: 1410.1465. Such a base change is particularly advantageous because it makes it possible to significantly accelerate the aging treatment.
    [0024] The adjustment may thus include the following calculations: ## EQU1 ## 01.3 1 0 el 01.3 1 0 1) 013 0 1 J 01.3 0 1 èii. = éTr é, .----> é, + AT. ## EQU1 ## ## EQU1 ## + At 01.3 1 0 01.3 1 0 é 01.3 1 0 01.3 0 1 01.3 0 1 J 01.3 0 In these equations, - fg denotes the integral of the calculated gravity vector in an estimated position of the carrier between instants t and t + Δt, - ffg denotes the double integral of the gravity vector calculated on the estimated position between the instants t and t + M - eR ', e, e; adjusted first terms, - In denotes an identity block of order n, and - 0 'denotes a square block of order n whose terms are nil The first adjusted correction terms eR, 4, e are used as data input to step 505 producing the second correction terms by reference change An aging transformation can alternatively (or in addition) be applied to at least one linear correction term resulting from the multiplication of the Z innovation and the gain from Kalman K.
    [0025] The aging transformation applied to a linear correction term e may be of the following form: E -> where cl) is defined as above. The same trick involving matrices ci) bd, Ad (), Ad (jit + At) and P can also be used.
    [0026] Two successive aging treatments implemented during two successive iterations of the Kalman filter may, in one embodiment, be independent of one another. However, in another embodiment, an aging process implemented during a given iteration of the Kalman filter over a corrective term is dependent on previous ages regarding the same corrective term. In this case, the processor has, during a preceding iteration: applied an approximate aging transformation to the corrective term considered, calculated a difference representing an approximation error in the implementation of this approximate aging transformation, - memorized this gap.
    [0027] It is understood that this principle can be generalized to N previous iterations: it can thus be stored a difference representing a cumulative approximation error in the implementation of an aging transformation of these previous N iteration of the extended Kalman filter. At the current iteration (which follows the previous N iterations), an approximate aging transformation is applied again at the corrective end to be adjusted. However, this corrective term is subject to an additional correction taking into account the cumulative difference stored during the previous N iterations.
    [0028] The calculation of the difference between the aged corrected term and its approximation can take the form of a difference in the sense of a group law, a vector difference, or a vector difference corrected by additional terms derived from from the BakerCampbell-Hausdorff formula.
    [0029] Such an embodiment provides some flexibility in implementing the adjustment step, which aims to age a corrective term involved in the extended Kalman filter update calculations. Indeed, it is possible that the theoretically optimal aging treatment can not be conducted immediately. It is then preferred to implement an approximate aging treatment, therefore less accurate, but without delay, while memorizing the error induced by this approximation for later use. Aging Secondary Measurements In some hybrid navigation architecture architectures, the start time of update calculations may be constrained so that there may be a significant delay between the time a new secondary metric is received ( for example GNSS in the case of hybrid inertial / GNSS plants) and the instant at which the updating calculations using this new measurement are actually started (this phenomenon being also illustrated in FIG. 4). The secondary measures provided are therefore in this case already obsolete at the very moment when the update calculations are started. This obsolescence can be corrected by an aging treatment applied to the last secondary measurement Y obtained. According to a first variant, the aging treatment applied to the last received secondary measurement comprises a time extrapolation of the last secondary measurement received at the subsequent start time of the update calculations.
    [0030] This extrapolation may depend on previous (s) secondary measurement (s) and / or on at least one state previously estimated by the extended Kalman filter. According to a second variant, the aging treatment applied to the last received secondary measurement comprises a modification of the observation function used to update the covariance matrix P.
    权利要求:
    Claims (16)
    [0001]
    REVENDICATIONS1. A mobile carrier navigation tracking method, wherein an extended Kalman filter (EKF) estimates during successive iterations a carrier's navigation state, an iteration of the filter (EKF) comprising steps of: - propagation (300) a previous navigation state of the carrier in a propagated state according to a kinematic model and / or measurements acquired by at least one inertial sensor (11), 10 - update (500) of the state propagated according to measurements acquired by at least one navigation sensor (12), the method being characterized in that, for at least one carrier navigation variable contained in the propagated state, the update (500) comprises substeps of: - computation (504) of a first corrective term expressed in a fixed reference (Rp) relative to the carrier, - calculation (505) of a second corrective term expressed in an inertial reference (Ri) in which the carrier is mobile, by a change of reference applied to the first corrective term, 20 - adding (506) the second corrective term to the value of the variable contained in the propagated state, the state containing the result of this addition being used as the output state of the iteration.
    [0002]
    2. Method according to the preceding claim, wherein the update (500) further comprises substeps of: - calculating (503) a linear correction term from an innovation representative of a deviation between your measurements acquired by the navigation sensor (12) and the propagated state, calculating an exponential of the linear correction in the sense of a Lie group, the first corrective term depending on said exponential. 3034514 24
    [0003]
    3. Method according to the preceding claim, wherein the linear correction is equal to the innovation multiplied by a gain of Kalman.
    [0004]
    4. Method according to one of the preceding claims, wherein the computation (505) of the second corrective term comprises a multiplication of a rotation matrix with te first corrective term.
    [0005]
    5. The method according to one of the preceding claims, comprising an adjustment of at least one of the corrective terms, before its use by the filter, by means of an aging transformation of said corrective term during the duration of an iteration of the extended Kalman filter.
    [0006]
    6. Method according to the preceding claim, wherein the adjustment of the corrective term comprises substeps of: additional correction of the correction term from a stored deviation representing a cumulative approximation error in the implementation of an aging transformation of at least one previous iteration of the filter, - application of the aging transformation to the offset corrected term.
    [0007]
    7. Method according to one of claims 5 and 6, wherein the adjustment of the first corrective term is implemented by means of a transformation comprising a base change of the first corrective term, the destination base being invariant by the action of a group.
    [0008]
    8. Method according to one of claims 5 to 7, wherein the adjustment comprises a transformation of the linear correction term E of the following form: E -> cPe 3034514 where (1) is a matrix of which at least one block is calculated by the formula Ad (Xt + At) - 1PAd (Xt), Ad (.) being an adjoint matrix in the sense of Lie group theory, i is a propagation matrix, and At a duration of iteration of the extended Kalman filter.
    [0009]
    9. Process according to the preceding claim, in which F has one of the following forms: ## STR3 ## OR 5f g AT. 13 13 - = a 03.3) I; or f g 13 - '= 16; where - f g denotes the integral of the gravity vector calculated at an estimated position of the wearer between your instants t and t + At; 5f g designates the double integral of the gravity vector calculated at an estimated position of the wearer between the instants t and t + At; - / 3 denotes an identity block of order 3, - 03,3 denotes a square block of order 3 whose terms are zero.
    [0010]
    10. Method according to one of the preceding claims, wherein the propagation implements a Riccati equation of the following form: = (te te where cl) is a matrix whose at least one of the blocks is calculated by the formulaAdUt + At) -1-tAd (gt), Ad (.) Being an adjoint matrix in the sense of Lie group theory, F is a propagation matrix, and At 25 is an iteration duration of the Kalman filter extended.
    [0011]
    The method according to one of the preceding claims, whose steps are implemented by a plurality of Kalman filters configured with different initial conditions, and operating in parallel so as to produce a plurality of candidate estimates of the carrier's navigation state, further comprising generating a consolidated estimate of the carrier's navigation state from the plurality of candidate estimates, the consolidated estimate being dependent on a comparison between each estimate candidate and the measurements acquired by the navigation sensor (12). 10
    [0012]
    12. Method according to one of the preceding claims, wherein at least one navigation sensor (12) is a receiver of radionavigation signals emitted by satellites. 15
    [0013]
    A computer program product comprising program code instructions for executing the steps of the method according to one of the preceding claims, when the program is executed by a computer. 20
    [0014]
    14. A mobile carrier navigation tracking device (2) comprising: an interface (21) for receiving measurements acquired by at least one inertial sensor (11); an interface (22) for receiving acquired secondary measurements; by at least one navigation sensor (12), - at least one processor (20) configured to implement an extended Kalman filter during successive iterations to estimate a navigation state of the carrier, an iteration of the filter comprising steps of: o propagating a previous navigation state into a propagated state based on a kinematic model and / or primary measurements, 3034514 27 o updating the propagated state according to the navigation measurements, the device (2) being characterized in that the processor (20) is further configured to update at least one propagated state navigation variable with sub-steps of: - calculating a first corrective term expressed in a fixed landmark by bearer report, - calculation of a second corrective term expressed in an inertial frame in which the carrier is mobile, by a change of mark 10 applied to the first corrective term, - addition of the second corrective term to the value of the variable in the propagated state, the state containing the result of this addition being used as the output state of the iteration. 15
    [0015]
    15. Navigation unit (1) for a mobile carrier, comprising: - at least one inertial sensor (11), - at least one navigation sensor (12), and - at least one navigation tracking device (10) according to the preceding claim. 20
    [0016]
    16. Navigation unit (1) according to the preceding claim, wherein at least one navigation sensor (12) is a receiver of radionavigation signals emitted by satellites. 25
    类似技术:
    公开号 | 公开日 | 专利标题
    EP3278061B1|2019-09-18|Method for tracking the navigation of a mobile carrier with an extended kalman filter
    FR3028031B1|2019-09-20|METHOD FOR ESTIMATING A NAVIGATION STATUS CONSTRAINED IN OBSERVABILITY
    KR20180052075A|2018-05-17|A navigation system
    KR102340630B1|2021-12-20|A navigation system
    EP3071934B1|2018-01-03|Alignment procedure of an inertial measuring unit
    EP2428934A1|2012-03-14|Method for estimating the movement of a carrier in relation to an environment and calculation device for a navigation system
    EP2241856A1|2010-10-20|Hybrid inertial system with non-linear behaviour and associated hybridising method using multi-hypothesis filtering
    WO2015058986A1|2015-04-30|Method for indoor and outdoor positioning and portable device implementing such a method
    El-Shafie et al.2014|Amplified wavelet-ANFIS-based model for GPS/INS integration to enhance vehicular navigation system
    CN105910617B|2019-01-15|The method and device of sampled point time synchronization in a kind of onboard navigation system
    EP3447654A1|2019-02-27|Method for determining the trajectory of a moving object, program and device for implementing said method
    WO2016005532A1|2016-01-14|Invariant particle filter
    EP3655724B1|2021-05-05|Method for estimating the movement of an object moving in a magnetic field
    JP6178776B2|2017-08-09|Speed measuring device
    Cucci et al.2017|A general approach to time-varying parameters in pose-graph optimization
    FR3104705A1|2021-06-18|PARTICULAR FILTERING AND NAVIGATION CENTRAL WITH MEASUREMENT CORRELATION
    WO2020233906A1|2020-11-26|Particle filtering method and navigation system using measurement correlation
    FR3104704A1|2021-06-18|PARTICULAR FILTERING AND NAVIGATION CENTRAL WITH MEASUREMENT CORRELATION
    Ran et al.2022|SINS/GPS Tightly-Coupled Integration Navigation Method Based on Quadrature Point Updating Cubature Kalman Filter
    WO2016189078A1|2016-12-01|Method for controlling the precession of a vibrating gyroscope
    WO2018069191A1|2018-04-19|Improvements to methods for aligning inertial navigation systems
    Jaiswal2013|Kalman Filter: Tracking theif path using Kalmar recursive approach
    同族专利:
    公开号 | 公开日
    WO2016156602A1|2016-10-06|
    EP3278061A1|2018-02-07|
    CN107690567A|2018-02-13|
    US20180095159A1|2018-04-05|
    EP3278061B1|2019-09-18|
    US10345427B2|2019-07-09|
    CN107690567B|2019-01-15|
    FR3034514B1|2017-04-21|
    引用文献:
    公开号 | 申请日 | 公开日 | 申请人 | 专利标题
    WO2005071431A1|2004-01-23|2005-08-04|Novatel Inc.|Inertial gps navigation system with modified kalman filter|
    EP1801539A1|2005-12-20|2007-06-27|Thales|Closed loop hybridisation device with surveillance of measurement integrity|
    US7193559B2|2003-01-21|2007-03-20|Novatel, Inc.|Inertial GPS navigation system with modified kalman filter|
    FR2944101B1|2009-04-07|2011-06-03|Thales Sa|NON-LINEAR BEHAVIOR HYBRID INERTIAL SYSTEM AND ASSOCIATED MULTIPURPOSE FILTRATION HYBRIDIZATION METHOD|
    US9052202B2|2010-06-10|2015-06-09|Qualcomm Incorporated|Use of inertial sensor data to improve mobile station positioning|
    FR2961897B1|2010-06-25|2012-07-13|Thales Sa|NAVIGATION FILTER FOR A FIELD CORRELATION NAVIGATION SYSTEM|
    CN102519463A|2011-12-13|2012-06-27|华南理工大学|Navigation method and device based on extended Kalman filter|
    US8515672B1|2012-03-01|2013-08-20|Honeywell International Inc.|Systems and methods to incorporate master navigation system resets during transfer alignment|
    US9223007B2|2012-11-21|2015-12-29|Raytheon Company|Kalman filtering with indirect noise measurements|
    CN103471595B|2013-09-26|2016-04-06|东南大学|A kind of iteration expansion RTS mean filter method towards the navigation of INS/WSN indoor mobile robot tight integration|
    FR3013829B1|2013-11-22|2016-01-08|Sagem Defense Securite|METHOD FOR ALIGNING AN INERTIAL PLANT|
    CN103983263A|2014-05-30|2014-08-13|东南大学|Inertia/visual integrated navigation method adopting iterated extended Kalman filter and neural network|CN108318027B|2017-01-18|2020-09-01|腾讯科技（深圳）有限公司|Method and device for determining attitude data of carrier|
    WO2019240555A1|2018-06-15|2019-12-19|주식회사 엘지화학|Decoration member|
    CN110186480B|2019-05-30|2021-03-26|北京航天控制仪器研究所|Method for determining error coefficient of linear system of inertial device|
    CN111399023B|2020-04-20|2022-02-08|中国人民解放军国防科技大学|Inertial basis combined navigation filtering method based on lie group nonlinear state error|
    CN112987560B|2021-04-19|2021-09-10|长沙智能驾驶研究院有限公司|Filter control method, device, equipment and computer storage medium|
    法律状态:
    2016-03-22| PLFP| Fee payment|Year of fee payment: 2 |
    2016-10-07| PLSC| Search report ready|Effective date: 20161007 |
    2017-03-22| PLFP| Fee payment|Year of fee payment: 3 |
    2018-03-02| CD| Change of name or company name|Owner name: ASSOCIATION POUR LA RECHERCHE DEVELOPPEMENT DE, FR Effective date: 20180126 Owner name: SAFRAN ELECTRONICS & DEFENSE, FR Effective date: 20180126 |
    2018-03-02| CJ| Change in legal form|Effective date: 20180126 |
    2018-03-22| PLFP| Fee payment|Year of fee payment: 4 |
    2020-03-19| PLFP| Fee payment|Year of fee payment: 6 |
    2021-03-23| PLFP| Fee payment|Year of fee payment: 7 |
    优先权:
    申请号 | 申请日 | 专利标题
    FR1500654A|FR3034514B1|2015-04-01|2015-04-01|NAVIGATION TRACKING METHOD OF A MOBILE CARRIER WITH AN EXTENDED KALMAN FILTER|FR1500654A| FR3034514B1|2015-04-01|2015-04-01|NAVIGATION TRACKING METHOD OF A MOBILE CARRIER WITH AN EXTENDED KALMAN FILTER|
    EP16715279.2A| EP3278061B1|2015-04-01|2016-04-01|Method for tracking the navigation of a mobile carrier with an extended kalman filter|
    US15/563,262| US10345427B2|2015-04-01|2016-04-01|Method for tracking the navigation of a mobile carrier with an extended kalman filter|
    PCT/EP2016/057280| WO2016156602A1|2015-04-01|2016-04-01|Method for tracking the navigation of a mobile carrier with an extended kalman filter|
    CN201680029288.2A| CN107690567B|2015-04-01|2016-04-01|The method for being used to be tracked the navigation of mobile vehicle equipment using extended Kalman filter|
    [返回顶部]